/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.ortn.wildbots.subsystems;

import edu.wpi.first.wpilibj.Jaguar;
import edu.ortn.wildbots.RobotMap;
import edu.ortn.wildbots.commands.CameraControl;
import edu.wpi.first.wpilibj.DigitalInput;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Subsystem;
/**
 *
 * @author Stephen
 */
public class Launcher extends Subsystem{
    
    private Jaguar launcher;
    private Encoder encoder;
    private double vTemp = 10; //meters
    private double lowBound = 0;
    private double highBound = 20;
    private double vTemp2;
    private final int THETA = 45;
    private final int G = 10;
    private boolean found = false;
    
    private double speed;
    
    public Launcher() {
        launcher = new Jaguar(RobotMap.LAUNCHER_MOTOR_PORT);
        encoder = new Encoder(RobotMap.ENCODER_PORT_A, RobotMap.ENCODER_PORT_B);
    }
    
    public void initDefaultCommand() {
    
    }
    
    public void startMotor(int num) {
       
    }
    
    public void stopMotor(int num) {
        
    }
    
    public double getVelocity(double distance, int alpha){
        while( found == false){
        if (-vTemp* vTemp*distance*Math.tan(Math.toRadians(vTemp))+ 
                vTemp*vTemp*vTemp*Math.sin(Math.toRadians(vTemp)) + 
                G*distance/Math.cos(Math.toRadians(vTemp))/2 >lowBound){
             if (-vTemp* vTemp*distance*Math.tan(Math.toRadians(vTemp))+ 
                vTemp*vTemp*vTemp*Math.sin(Math.toRadians(vTemp)) + 
                G*distance/Math.cos(Math.toRadians(vTemp))/2 >-.1 &&-vTemp* vTemp*distance*Math.tan(Math.toRadians(vTemp))+ 
                vTemp*vTemp*vTemp*Math.sin(Math.toRadians(vTemp)) + 
                G*distance/Math.cos(Math.toRadians(vTemp))/2 <.1){
                 return vTemp;
             }
             else{
                 //-*+9 9+++++++++++++++++++++++++++++++++++++++++++++++++++++vTemp;
             }
        }
        }
        return vTemp;
    }
    
    public void update(){
       adjustSpeed();        
    }
    
    public void reset() {
      
    }
    
    public void fire(){
     
    }
    
    public void setLauncherEnabled(boolean yes){
        double speed = launcherSpeed(CameraControl.getVerticleAngle());
        if(yes){
            speed = 0.5;
        }else{
            speed = 0;
        }
    }
    
    public double launcherSpeed(double camAngle){
        double x = 2.0;
        //Sean's formula will go here(camAngle) -> x
        return x;
    }
    
    public void setSpeed(double speed) {
        this.speed = speed;
    }
    
    public void adjustSpeed(){
//        double x = 1.0;
//        double timeCount = 0.0;
//        double count = 0.0;
//        encoder.reset();
//        while(encoder.get() < 1){
//            Timer.delay(0.001);
//            timeCount += 0.001;
//        }
//        
//        double percentage = x/speed;
//        while(percentage > 1.05){
//            launcher.set(launcher.getSpeed() - 0.05);
//        }
        boolean isPositive = (speed - launcher.getSpeed()) > 0;
        if (Math.abs(launcher.getSpeed() - speed) < .001) {
            if (isPositive)
                launcher.set(launcher.getSpeed() + 0.1);
            else
                launcher.set(launcher.getSpeed() - 0.1);
        }
    }
}

    

